﻿#include "spline.h"
#include <cmath>
#include <fstream>
#include <istream>

#include <Eigen/Dense>
using namespace std;

namespace oil
{
Spline::Spline(const PointCloudPtr& input, double lambda)
	: lambda_(lambda)
{
	setInputCloud(input);
	calcTPS();
}


Spline::~Spline(void)
{
}

void Spline::calcTPS()
{
	int p = input_->size(); //点数目

	MatrixXd A = MatrixXd::Zero(p + 3, p + 3);
	VectorXd x = VectorXd::Zero(p + 3);
	VectorXd b = VectorXd::Zero(p + 3);

	//1. 计算alpha
	double alpha = 0.0;

	//2. Kij
	for (int i = 0; i < p; ++i)
	{
		for (int j = i + 1; j < p; ++j)
		{
			const PointXYZ& pt_i = input_->points[i];
			const PointXYZ& pt_j = input_->points[j];
			double rij = sqrt((pt_i.x - pt_j.x) * (pt_i.x - pt_j.x) +
				(pt_i.y - pt_j.y) * (pt_i.y - pt_j.y));
			alpha += rij * 2;
			A(i, j) = radialFunction(rij);
		}
	}
	alpha /= double(p*p);

	//3. K的对角线元素
	//4. 计算P , b 和 K的对角线元素 
	for (int i = 0; i < p; ++i)
	{
		A(i, i) = alpha * alpha * lambda_;

		A(i, p) = 1.0;
		A(i, p + 1) = input_->points[i].x;
		A(i, p + 2) = input_->points[i].y;
		b(i) = input_->points[i].z;
	}

	//5. 拷贝下三角元素
	A.triangularView<Lower>() = A.triangularView<Upper>().transpose();

	//6. solve Ax=b;	
	x = A.householderQr().solve(b);
	v_ = x;

	//7. bending energy
	VectorXd w = x.head(p);
	MatrixXd K = A.topLeftCorner(p, p);
	bending_ = (w.transpose() * K * w)(0, 0);

	//8. cost
// 	VectorXd v_cost = VectorXd::Zero(p);
// 	for (int i = 0; i < p; ++i)
// 	{
// 		PointXYZ pt = input_->points[i];
// 		double z = pt.z;
// 		interpolate(pt);
// 		v_cost(i) = z - pt.z;
// 	}
// 	double mean;
// 	v_cost.meanAndStdAll(mean, std_);
// 	cost_ = sqrt((v_cost.transpose() * v_cost)(0, 0) / p);
}

void Spline::interpolate(PointCloudPtr& cloud)
{
	for (int i = 0; i < cloud->points.size(); ++i)
	{
		PointXYZ point = cloud->points[i];
		interpolate(point);
		cloud->points[i] = point;
	}
}
void Spline::interpolate(PointXYZ& point)
{
	double x, y, z;
	x = point.x; y = point.y;
	z = 0.0;
	int p = input_->size();
	z = v_(p + 0) + v_(p + 1) * x + v_(p + 2) * y;
	for (int i = 0; i < input_->size(); ++i)
	{
		PointXYZ pi = input_->points[i];
		double ri = sqrt((x - pi.x) * (x - pi.x) + (y - pi.y) * (y - pi.y));
		ri = radialFunction(ri);
		z += v_[i] * ri;
	}
	point.z = z;
}
void Spline::interpolate(PointCloud& points)
{
	for (int i = 0; i < points.size(); ++i)
	{
		PointXYZ point = points[i];
		interpolate(point);
		points[i] = point;
	}
}

double Spline::radialFunction(double r)
{
	if (r == 0.0)
		return 0.0;
	return (r*r*log(r));
}

}//end oil
